#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "cloth.h"
#include "argparser.h"
#include <fstream>
#include "vectors.h"

using namespace std;

// ================================================================================
// some helper drawing functions

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);
void DrawForce(const Vec3f &p, const Vec3f &f);

// ================================================================================
// ================================================================================

Cloth::Cloth(ArgParser *_args) {
  args =_args;

  // open the file
  ifstream istr(args->cloth_file.c_str());
  assert (istr != NULL);
  string token;

  // read in the simulation parameters
  istr >> token >> k_structural; assert (token == "k_structural");  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");
  // NOTE: correction factor == .1, means springs shouldn't stretch more than 10%
  //       correction factor == 100, means don't do any correction
  istr >> token >> provot_structural_correction; assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; assert (token == "provot_shear_correction");

  // the cloth dimensions
  istr >> token >> nx >> ny; // (units == meters)
  assert (token == "m");
  assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  Vec3f a,b,c,d;
  istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  istr >> token >> fabric_weight; assert (token == "fabric_weight");
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);

  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) {
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
    }
  }

  // the fixed particles
  while (istr >> token) {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }

  computeBoundingBox();

  //Store particle spring connections
  for(int i = 0; i < nx; i++)
  {
	  for(int j = 0; j < ny; j++)
	  {
		  ClothParticle &p = getParticle(i,j);
		  if(i < nx - 1)
		  {
			  p.v_structural.push_back(std::make_pair(i+1, j));
			  ClothParticle &tempP = getParticle(i+1, j);
			  tempP.v_structural.push_back(std::make_pair(i, j));
		  }

		  if(j < ny - 1)
		  {
			  p.v_structural.push_back(std::make_pair(i, j+1));
			  ClothParticle &tempP = getParticle(i, j+1);
			  tempP.v_structural.push_back(std::make_pair(i, j));
		  }

		  if(i < nx - 1 && j < ny - 1)
		  {
			  p.v_shear.push_back(std::make_pair(i+1, j+1));
			  ClothParticle &tempP = getParticle(i+1, j+1);
			  tempP.v_shear.push_back(std::make_pair(i, j));

			  ClothParticle &tempP2 = getParticle(i+1, j);
			  tempP2.v_shear.push_back(std::make_pair(i, j+1));
			  ClothParticle &tempP3 = getParticle(i, j+1);
			  tempP3.v_shear.push_back(std::make_pair(i+1, j));
		  }

		  if(i < nx - 2)
		  {
			  p.v_bend.push_back(std::make_pair(i+2, j));
			  ClothParticle &tempP = getParticle(i+2, j);
			  tempP.v_bend.push_back(std::make_pair(i, j));
		  }

		  if(j < ny - 2)
		  {
			  p.v_bend.push_back(std::make_pair(i, j+2));
			  ClothParticle &tempP = getParticle(i, j+2);
			  tempP.v_bend.push_back(std::make_pair(i, j));
		  }
	  }
  }
}

// ================================================================================

void Cloth::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
}

// ================================================================================
// ================================================================================
// ================================================================================

void Cloth::Paint() const {

  // =====================================================================================
  // render the bounding box
  // =====================================================================================
  if (args->bounding_box) 
    box.Paint();

  // =====================================================================================
  // render the particles
  // =====================================================================================

  if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
	const ClothParticle &p = getParticle(i,j);
	const Vec3f &pos = p.getPosition();
	bool fixed = p.isFixed();
	if (fixed) 
	  glColor3f(0,1,0);
	else
	  glColor3f(0,0,0);	
	glVertex3f(pos.x(),pos.y(),pos.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the velocity at each particle
  // =====================================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the force at each particle
  // =====================================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);


	// ASSIGNMENT:
	// visualize the forces acting on each particle
		for(unsigned int k = 0; k < p.forces.size(); k++)
		{
			glColor3f(0,0,1);
			DrawForce(p.getPosition(), p.forces[k]);
		}
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the cloth surface
  // =====================================================================================

  if (args->surface) {
    glColor3f(1,1,1);
    glBegin(GL_QUADS);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
	const Vec3f &a = getParticle(i,j).getPosition();
	const Vec3f &b = getParticle(i,j+1).getPosition();
	const Vec3f &c = getParticle(i+1,j+1).getPosition();
	const Vec3f &d = getParticle(i+1,j).getPosition();
	insertNormal(a,b,c,d);
	glVertex3f(a.x(),a.y(),a.z());
	glVertex3f(b.x(),b.y(),b.z());
	glVertex3f(c.x(),c.y(),c.z());
	glVertex3f(d.x(),d.y(),d.z());
      }
    }
    glEnd();
  }

  // =====================================================================================
  // render the wireframe cloth (the structural and shear springs)
  // =====================================================================================

  if (args->wireframe) {
    glDisable(GL_LIGHTING);
    glLineWidth(1);
    glBegin(GL_LINES);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        const Vec3f &a_o = getParticle(i,j).getOriginalPosition();
        const Vec3f &b_o = getParticle(i,j+1).getOriginalPosition();
        const Vec3f &c_o = getParticle(i+1,j+1).getOriginalPosition();
        const Vec3f &d_o = getParticle(i+1,j).getOriginalPosition();        
        const Vec3f &a = getParticle(i,j).getPosition();
        const Vec3f &b = getParticle(i,j+1).getPosition();
        const Vec3f &c = getParticle(i+1,j+1).getPosition();
        const Vec3f &d = getParticle(i+1,j).getPosition();        
        DrawSpring(a_o,b_o,a,b, provot_structural_correction);
        DrawSpring(a_o,c_o,a,c, provot_shear_correction);
        DrawSpring(a_o,d_o,a,d, provot_structural_correction);
        DrawSpring(b_o,c_o,b,c, provot_structural_correction);
        DrawSpring(b_o,d_o,b,d, provot_shear_correction);
        DrawSpring(c_o,d_o,c,d, provot_structural_correction);
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  }
}

// ================================================================================
// ================================================================================

void Cloth::Animate() {


  // ASSIGNMENT:
  // move the vertices of the mesh
	Vec3f iPos, iVel, iAcc;
	Vec3f totalForce;
	Vec3f origL, l;
	float h = args->timestep;

	for(int i = 0; i < nx; i++)
	{
		for(int j = 0; j < ny; j++)
		{
			ClothParticle &p = getParticle(i, j);
			p.forces.clear();
			totalForce = args->gravity * p.getMass(); //Start totalForce with gravity
			p.forces.push_back(args->gravity * p.getMass()); //Forces is used to draw forces acting on a particle later
			
			//Starts with structural
			for(unsigned int k = 0; k < p.v_structural.size(); k++)
			{
				ClothParticle &tempP = getParticle(p.v_structural[k].first,  p.v_structural[k].second);
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();
				//If stretched too far
				if (l.Length() > (1+provot_structural_correction) * origL.Length())
				{
					float diff = l.Length() - (1+provot_structural_correction) * origL.Length();
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed())
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed())
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed())
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}

				//if pushed too close
				else if(l.Length() < (1-provot_structural_correction) * origL.Length())
				{
					float diff = (1-provot_structural_correction) * origL.Length() - l.Length();
					diff = diff * -1;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed())
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed())
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed())
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}

				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_structural * len;
				l.Normalize();
				l = l * len;
				totalForce += l;
				p.forces.push_back(l);
			}

			//Do similar setup for shear, except use the proper correction and k
			for(unsigned int k = 0; k < p.v_shear.size(); k++)
			{
				ClothParticle &tempP = getParticle(p.v_shear[k].first,  p.v_shear[k].second);
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();

				if (l.Length() > (1+provot_shear_correction) * origL.Length())
				{
					float diff = l.Length() - (1+provot_shear_correction) * origL.Length();
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed())
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed())
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed())
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}

				else if(l.Length() < (1-provot_shear_correction) * origL.Length())
				{
					float diff = (1-provot_shear_correction) * origL.Length() - l.Length();
					diff = diff * -1;
					Vec3f tempPoint = l;
					Vec3f tempPoint2 = p.getPosition() - tempP.getPosition();
					tempPoint.Normalize();
					tempPoint2.Normalize();

					if(!p.isFixed() && !tempP.isFixed())
					{
						tempPoint = tempPoint * (diff / 2);
						tempPoint2 = tempPoint2 * (diff / 2);
						tempPoint += p.getPosition();
						tempPoint2 += tempP.getPosition();
						p.setPosition(tempPoint);
						tempP.setPosition(tempPoint2);
					}

					else if(!p.isFixed())
					{
						tempPoint = tempPoint * diff;
						tempPoint += p.getPosition();
						p.setPosition(tempPoint);
					}

					else if (!tempP.isFixed())
					{
						tempPoint2 = tempPoint2 * diff;
						tempPoint2 += tempP.getPosition();
						tempP.setPosition(tempPoint2);
					}
				}

				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_shear * len;
				l.Normalize();
				l = l * len;
				totalForce += l;
				p.forces.push_back(l);
			}

			//And now for bend, which doesn't need the correctional setup
			for(unsigned int k = 0; k < p.v_bend.size(); k++)
			{
				ClothParticle &tempP = getParticle(p.v_bend[k].first,  p.v_bend[k].second);
				origL = tempP.getOriginalPosition() - p.getOriginalPosition();
				l = tempP.getPosition() - p.getPosition();
				float len = origL.Length() - l.Length();
				len = -k_bend * len;
				l.Normalize();
				l = l * len;
				totalForce += l;
				p.forces.push_back(l);
			}

			if(!p.isFixed())
			{
				iPos = p.getPosition();
				iVel = p.getVelocity();
	

				totalForce = totalForce - p.getVelocity() * damping;
				p.setPosition(iPos + (h * p.getVelocity()));
				p.setVelocity(iVel + (h * p.getAcceleration()));
				p.setAcceleration(totalForce * (1 / p.getMass()));	
			}
		}
	}
}


// ================================================================================
// ================================================================================
// some helper drawing functions
// ================================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ================================================================================
// ================================================================================

